#include "vsf.h"
#include "vsf_arduino.h"

static void vsf_arduino_run(struct vsfsm_thread_t *thread)
{
	struct vsf_arduino_t *arduino = (struct vsf_arduino_t *)thread->priv;
	arduino->setup();
	while (1)
		arduino->loop();
}

vsf_err_t vsf_arduino_start(struct vsf_arduino_t *arduino)
{
	memset(&arduino->thread, 0, sizeof(arduino->thread));
	arduino->thread.priv = arduino;
	arduino->thread.op.on_run = vsf_arduino_run;
	arduino->thread.stack_size = sizeof(arduino->stack);
	arduino->thread.stack = arduino->stack;
	return vsfsm_thread_start(&arduino->thread);
}

#include "Arduino.h"
void pinMode(uint8_t pin, uint8_t mode)
{
	uint8_t port = digitalPinToPort(pin);
	uint32_t mode32;

	vsfhal_gpio_init(port);
	switch (mode)
	{
	case INPUT:			mode32 = VSFHAL_GPIO_INPUT; break;
	case OUTPUT:		mode32 = VSFHAL_GPIO_OUTPP; break;
	case INPUT_PULLUP:	mode32 = VSFHAL_GPIO_INPUT | VSFHAL_GPIO_PULLUP; break;
	}
	vsfhal_gpio_config(port, pin & 0x1F, mode32);
}

void digitalWrite(uint8_t pin, uint8_t value)
{
	uint8_t port = digitalPinToPort(pin);
	uint32_t pin_mask = digitalPinToBitMask(pin);
	vsfhal_gpio_out(port, pin_mask, value ? pin_mask : 0);
}

int digitalRead(uint8_t pin)
{
	uint8_t port = digitalPinToPort(pin);
	uint32_t pin_mask = digitalPinToBitMask(pin);
	return vsfhal_gpio_get(port, pin_mask);
}

unsigned long millis(void)
{
	return vsfhal_tickclk_get_ms();
}

unsigned long micros(void)
{
	return vsfhal_tickclk_get_us();
}

// TODO: implement accurate delay
void delay(unsigned long ms)
{
	struct vsfsm_t *sm = &(vsfsm_thread_get_cur())->sm;
	vsftimer_create(sm, ms, 1, VSFSM_EVT_USER);
	vsfsm_thread_wfe(VSFSM_EVT_USER);
}

void delayMicroseconds(unsigned int us)
{
	
}
